#ifndef __BLEND_INLINE__
#define __BLEND_INLINE__

#include "xGraphic_Type.h"

#include <mmintrin.h>
#include <xmmintrin.h>
#include <emmintrin.h>

#define MASK_SUB 0x0100010001000100
#define MASK_ADD 0x0001000100010001

__forceinline xARGB GetPixel32(PBITMAPEX lpBitmap, PBYTE pPixel)
{
    return *LPXARGB(pPixel);
}

__forceinline xARGB GetPixel24(PBITMAPEX lpBitmap, PBYTE pPixel)
{
    xARGB pixel;
    pixel.argb = (DWORD)(0xFF000000|((BYTE)(*pPixel)|((WORD)((BYTE)(*(pPixel + 1))) << 8))|(((DWORD)(BYTE)(*(pPixel + 2))) << 16));
    return pixel;
}

__forceinline xARGB GetPixel8(PBITMAPEX lpBitmap, PBYTE pPixel)
{
    return LPXARGB(lpBitmap->bmColors)[*pPixel];
}

__forceinline void MulAlpha(xARGB& src, DWORD alpha)
{
    src.argb = ((((src.argb & 0x00FF00FF) * alpha) & 0xFF00FF00) >> 8) + ((((src.argb & 0xFF00FF00) >> 8) * alpha) & 0xFF00FF00);
}

__forceinline void BlendPixelSrc(LPXARGB pDst, xARGB& src)
{
    *pDst = src;
}

__forceinline void BlendPixelSrcOver(LPXARGB pDst, xARGB& src)
{
    WORD alpha = 256 - src.alpha;
    pDst->argb = src.argb + ((((pDst->argb & 0x00FF00FF) * alpha) & 0xFF00FF00) >> 8) + ((((pDst->argb & 0xFF00FF00) >> 8) * alpha) & 0xFF00FF00);
}

__forceinline void BlendPixelMul(LPXARGB pDst, xARGB& src)
{
    pDst->alpha = (pDst->alpha * (src.alpha + 1)) >> 8;
    pDst->red   = (pDst->red   * (src.red   + 1)) >> 8;
    pDst->green = (pDst->green * (src.green + 1)) >> 8;
    pDst->blue  = (pDst->blue  * (src.blue  + 1)) >> 8;
}

__forceinline void BlendPixelDstOver(LPXARGB pDst, xARGB& src)
{
	WORD alpha = 256 - pDst->alpha;
    pDst->argb += ((((src.argb & 0x00FF00FF) * alpha) & 0xFF00FF00) >> 8) + ((((src.argb & 0xFF00FF00) >> 8) * alpha) & 0xFF00FF00);
}

__forceinline void BlendPixelOr(LPXARGB pDst, xARGB& src)
{
    pDst->argb |= src.argb;
}

__forceinline void BlendPixelGray(LPXARGB pDst, xARGB& src)
{
	WORD alpha = 256 - pDst->alpha;
	BYTE c = (pDst->blue  + pDst->green * 2 + pDst->red) >> 2;
    c = c * src.alpha >> 8;
    pDst->red   = c + ((alpha * pDst->red)   >> 8);
    pDst->green = c + ((alpha * pDst->green) >> 8);
    pDst->blue  = c + ((alpha * pDst->blue)  >> 8);
}

__forceinline void AddColorValue64(const xARGB &src, DWORD surf, UINT64 &b, UINT64 &g, UINT64 &r, UINT64 &a, UINT64 &s)
{
    b += src.blue * surf;
    g += src.green * surf;
    r += src.red * surf;
    a += src.alpha * surf;
    s += surf;
}

__forceinline void AddColorValue32(const xARGB &src, DWORD surf, DWORD &b, DWORD &g, DWORD &r, DWORD &a, DWORD &s)
{
    b += src.blue * surf;
    g += src.green * surf;
    r += src.red * surf;
    a += src.alpha * surf;
    s += surf;
}

__forceinline __m64 Get2Pixel32MMX(PBITMAPEX lpBitmap, PBYTE pPixel)
{
    return *(__m64*)pPixel;
}

__forceinline __m64 Get2Pixel24MMX(PBITMAPEX lpBitmap, PBYTE pPixel)
{
    return _mm_set_pi8((char)0xFF, *(pPixel+5), *(pPixel+4), *(pPixel+3), (char)0xFF, *(pPixel+2), *(pPixel+1), *(pPixel));
}

__forceinline __m64 Get2Pixel8MMX(PBITMAPEX lpBitmap, PBYTE pPixel)
{
    return _mm_set_pi32(LPXARGB(lpBitmap->bmColors)[*(pPixel+1)].argb, LPXARGB(lpBitmap->bmColors)[*pPixel].argb);
}

__forceinline void BlendPixelSrcMMX(DWORD& dst, DWORD src)
{
    dst = src;
}

__forceinline void Blend2PixelSrcMMX(__m64& dst, __m64& src)
{
    dst = src;
}

__forceinline void BlendPixelSrcOverMMX(DWORD& dst, DWORD src)
{
    __m64 msrc = _m_from_int(src);
    __m64 mdst = _m_from_int(dst);
    __m64 mzero = _mm_setzero_si64();
    __m64 alpha;
    __m64 subMask;
    subMask.m64_u64 = MASK_SUB;
    alpha = _m_punpcklwd(msrc, msrc);
    mdst = _m_punpcklbw(mdst, mzero);
    alpha = _m_punpckhdq(alpha, alpha);
    alpha = _m_psrlwi(alpha, 8);
    alpha = _m_psubw(subMask, alpha);
    mdst = _m_pmullw(mdst, alpha);
    mdst = _m_psrlwi(mdst, 8);
    mdst = _m_packuswb(mdst, mzero);
    mdst = _m_paddusb(msrc, mdst);
    dst = _m_to_int(mdst);
}

__forceinline void Blend2PixelSrcOverMMX(__m64& dst, __m64& src)
{
    __m64 src2 = src;
    __m64 dst2 = dst;
    __m64 mzero = _mm_setzero_si64();
    __m64 alpha1;
    __m64 alpha2;
    __m64 subMask;
    subMask.m64_u64 = MASK_SUB;
    alpha1 = _m_punpcklwd(src, src);
    alpha2 = _m_punpckhwd(src, src);
    dst = _m_punpcklbw(dst, mzero);
    dst2 = _m_punpckhbw(dst2, mzero);
    alpha1 = _m_punpckhdq(alpha1, alpha1);
    alpha2 = _m_punpckhdq(alpha2, alpha2);
    alpha1 = _m_psrlwi(alpha1, 8);
    alpha2 = _m_psrlwi(alpha2, 8);
    alpha1 = _m_psubw(subMask, alpha1);
    alpha2 = _m_psubw(subMask, alpha2);
    dst = _m_pmullw(dst, alpha1);
    dst2 = _m_pmullw(dst2, alpha2);
    dst = _m_psrlwi(dst, 8);
    dst2 = _m_psrlwi(dst2, 8);
    dst = _m_packuswb(dst, mzero);
    dst2 = _m_packuswb(dst2, mzero);
    dst2 = _m_psllqi(dst2, 32);
    dst = _m_pxor(dst, dst2);
    dst = _m_paddusb(src, dst);
}

__forceinline void BlendPixelDstOverMMX(DWORD& dst, DWORD src)
{
    __m64 msrc = _m_from_int(src);
    __m64 mdst = _m_from_int(dst);
    __m64 mzero = _mm_setzero_si64();
    __m64 alpha;
    __m64 subMask;
    subMask.m64_u64 = MASK_SUB;
    alpha = _m_punpcklwd(mdst, mdst);
    alpha = _m_punpckhdq(alpha, alpha);
    alpha = _m_psrlwi(alpha, 8);
    alpha = _m_psubw(subMask, alpha);
    msrc = _m_punpcklbw(msrc, mzero);
    msrc = _m_pmullw(msrc, alpha);
    msrc = _m_psrlwi(msrc, 8);
    msrc = _m_packuswb(msrc, mzero);
    mdst = _m_paddusb(mdst, msrc);
    dst = _m_to_int(mdst);
}

__forceinline void Blend2PixelDstOverMMX(__m64& dst, __m64& src)
{
    __m64 mzero = _mm_setzero_si64();
    __m64 alpha1;
    __m64 alpha2;
    __m64 subMask;
    subMask.m64_u64 = MASK_SUB;
    alpha1 = _m_punpcklwd(dst, dst);
    alpha2 = _m_punpckhwd(dst, dst);
    alpha1 = _m_punpckhdq(alpha1, alpha1);
    alpha2 = _m_punpckhdq(alpha2, alpha2);
    alpha1 = _m_psrlwi(alpha1, 8);
    alpha2 = _m_psrlwi(alpha2, 8);
    alpha1 = _m_psubw(subMask, alpha1);
    alpha2 = _m_psubw(subMask, alpha2);
    __m64 src1 = src;
    __m64 src2 = src;
    src1 = _m_punpcklbw(src1, mzero);
    src2 = _m_punpckhbw(src2, mzero);
    src1 = _m_pmullw(src1, alpha1);
    src2 = _m_pmullw(src2, alpha2);
    src1 = _m_psrlwi(src1, 8);
    src2 = _m_psrlwi(src2, 8);
    src1 = _m_packuswb(src1, mzero);
    src2 = _m_packuswb(src2, mzero);
    src2 = _m_psllqi(src2, 32);
    src1 = _m_pxor(src1, src2);
    dst = _m_paddusb(dst, src1);
}

__forceinline void BlendPixelMulMMX(DWORD& dst, DWORD src)
{
    __m64 msrc = _m_from_int(src);
    __m64 mdst = _m_from_int(dst);
    __m64 mzero = _mm_setzero_si64();
    __m64 addMask;
    addMask.m64_u64 = MASK_ADD;
    msrc = _m_punpcklbw(msrc, mzero);
    mdst = _m_punpcklbw(mdst, mzero);
    msrc = _m_paddusw(msrc, addMask);
    mdst = _m_pmullw(mdst, msrc);
    mdst = _m_psrlwi(mdst, 8);
    mdst = _m_packuswb(mdst, mzero);
    dst = _m_to_int(mdst);
}

__forceinline void Blend2PixelMulMMX(__m64& dst, __m64& src)
{
    __m64 src1;
    __m64 src2;
    __m64 dst2 = dst;
    __m64 mzero = _mm_setzero_si64();
    __m64 addMask;
    addMask.m64_u64 = MASK_ADD;
    src1 = _m_punpcklbw(src, mzero);
    src2 = _m_punpckhbw(src, mzero);
    dst = _m_punpcklbw(dst, mzero);
    dst2 = _m_punpckhbw(dst2, mzero);
    src1 = _m_paddusw(src1, addMask);
    src2 = _m_paddusw(src2, addMask);
    dst = _m_pmullw(dst, src1);
    dst2 = _m_pmullw(dst2, src2);
    dst = _m_psrlwi(dst, 8);
    dst2 = _m_psrlwi(dst2, 8);
    dst = _m_packuswb(dst, mzero);
    dst2 = _m_packuswb(dst2, mzero);
    dst2 = _m_psllqi(dst2, 32);
    dst = _m_pxor(dst, dst2);
}

__forceinline void BlendPixelOrMMX(DWORD& dst, DWORD src)
{
    dst |= src;
}

__forceinline void Blend2PixelOrMMX(__m64& dst, __m64& src)
{
    dst = _m_por(dst, src);
}

__forceinline void BlendPixelSrcOverMMX(DWORD& dst, DWORD src, __m64& alphaMask)
{
    __m64 msrc = _m_from_int(src);
    __m64 mdst = _m_from_int(dst);
    __m64 mzero = _mm_setzero_si64();
    __m64 alpha;
    __m64 subMask;
    subMask.m64_u64 = MASK_SUB;
    msrc = _m_punpcklbw(msrc, mzero);
    mdst = _m_punpcklbw(mdst, mzero);
    msrc = _m_pmullw(msrc, alphaMask);
    msrc = _m_psrlwi(msrc, 8);
    msrc = _m_packuswb(msrc, mzero);
    alpha = _m_punpcklwd(msrc, msrc);
    alpha = _m_punpckhdq(alpha, alpha);
    alpha = _m_psrlwi(alpha, 8);
    alpha = _m_psubw(subMask, alpha);
    mdst = _m_pmullw(mdst, alpha);
    mdst = _m_psrlwi(mdst, 8);
    mdst = _m_packuswb(mdst, mzero);
    mdst = _m_paddusb(msrc, mdst);
    dst = _m_to_int(mdst);
}

__forceinline void Blend2PixelSrcOverMMX(__m64& dst, __m64& src, __m64& alphaMask)
{
    __m64 src2 = src;
    __m64 dst2 = dst;
    __m64 mzero = _mm_setzero_si64();
    __m64 alpha1;
    __m64 alpha2;
    __m64 subMask;
    subMask.m64_u64 = MASK_SUB;
    src = _m_punpcklbw(src, mzero);
    src2 = _m_punpckhbw(src2, mzero);
    src = _m_pmullw(src, alphaMask);
    src2 = _m_pmullw(src2, alphaMask);
    src = _m_psrlwi(src, 8);
    src2 = _m_psrlwi(src2, 8);
    src = _m_packuswb(src, mzero);
    src2 = _m_packuswb(src2, mzero);
    src2 = _m_psllqi(src2, 32);
    src = _m_pxor(src, src2);
    dst = _m_punpcklbw(dst, mzero);
    dst2 = _m_punpckhbw(dst2, mzero);
    alpha1 = _m_punpcklwd(src, src);
    alpha2 = _m_punpckhwd(src, src);
    alpha1 = _m_punpckhdq(alpha1, alpha1);
    alpha2 = _m_punpckhdq(alpha2, alpha2);
    alpha1 = _m_psrlwi(alpha1, 8);
    alpha2 = _m_psrlwi(alpha2, 8);
    alpha1 = _m_psubw(subMask, alpha1);
    alpha2 = _m_psubw(subMask, alpha2);
    dst = _m_pmullw(dst, alpha1);
    dst2 = _m_pmullw(dst2, alpha2);
    dst = _m_psrlwi(dst, 8);
    dst2 = _m_psrlwi(dst2, 8);
    dst = _m_packuswb(dst, mzero);
    dst2 = _m_packuswb(dst2, mzero);
    dst2 = _m_psllqi(dst2, 32);
    dst = _m_pxor(dst, dst2);
    dst = _m_paddusb(src, dst);
}

__forceinline void BlendPixelSrcMMX(DWORD& dst, DWORD src, __m64& key)
{
    __m64 msrc = _m_from_int(src);
    __m64 mdst = _m_from_int(dst);
    __m64 mclr = msrc;
    msrc = _m_pcmpeqd(msrc, key);
    mclr = _m_psubusb(mclr, msrc);
    msrc = _m_pand(msrc, mdst);
    mdst = _m_por(msrc, mclr);
    dst = _m_to_int(mdst);
}

__forceinline void Blend4PixelSrcMMX(__m64& dst1, __m64& dst2, __m64& src1, __m64& src2, __m64& key)
{
    __m64 mclr1 = src1;
    __m64 mclr2 = src2;
    src1 = _m_pcmpeqd(src1, key);
    src2 = _m_pcmpeqd(src2, key);
    mclr1 = _m_psubusb(mclr1, src1);
    mclr2 = _m_psubusb(mclr2, src2);
    src1 = _m_pand(src1, dst1);
    src2 = _m_pand(src2, dst2);
    dst1 = _m_por(src1, mclr1);
    dst2 = _m_por(src2, mclr2);
}

__forceinline void AddColorValue64SSE2(const xARGB &src, DWORD surf, __m64 &b, __m64 &g, __m64 &r, __m64 &a, __m64 &s)
{
    __m64 v0 = _m_from_int(src.argb);
    __m64 v1 = _m_psrlqi(_m_from_int(surf), 8);
    __m64 v2 = _mm_setzero_si64();
    s = _mm_add_si64(s, v1);
    v1 = _m_punpcklwd(v1, v1);
    v0 = _m_punpcklbw(v0, v2);
    v1 = _m_punpckldq(v1, v1);
    v0 = _m_pmullw(v0, v1);
    v1 = _m_psrlqi(v0, 48);
    v2 = _m_psllqi(v0, 16);
    v2 = _m_psrlqi(v2, 48);
    a = _mm_add_si64(a, v1);
    r = _mm_add_si64(r, v2);
    v1 = _m_psllqi(v0, 32);
    v2 = _m_psllqi(v0, 48);
    v1 = _m_psrlqi(v1, 48);
    v2 = _m_psrlqi(v2, 48);
    g = _mm_add_si64(g, v1);
    b = _mm_add_si64(b, v2);
}

__forceinline void AddColorValue32SSE2(const xARGB &src, DWORD surf, __m64 &bg, __m64 &ra, DWORD &s)
{
    __m64 v0 = _m_from_int(src.argb);
    DWORD sv = surf >> 8;
    __m64 mzero = _mm_setzero_si64();
    s += sv;
    __m64 msurf = _m_from_int(sv);
    msurf = _m_punpcklwd(msurf, msurf);
    v0 = _m_punpcklbw(v0, mzero);
    msurf = _m_punpckldq(msurf, msurf);
    v0 = _m_pmullw(v0, msurf);
    __m64 v1 = v0;
    v0 = _m_punpckhwd(v0, mzero);
    v1 = _m_punpcklwd(v1, mzero);
    ra = _m_paddd(ra, v0);
    bg = _m_paddd(bg, v1);
}

#endif